/* 
 * scout_messenger.cpp
 * 
 * Created on: Apr 26, 2019 22:14
 * Description: 
 * 
 * Copyright (c) 2019 Ruixiang Du (rdu)
 */

#include "scout_base/scout_messenger.hpp"

#include <tf/transform_broadcaster.h>

#include "scout_msgs/ScoutStatus.h"



#include <thread>
#define GRAVITY_VALUE 9.78
namespace wescore
{
ScoutROSMessenger::ScoutROSMessenger(ros::NodeHandle *nh) : scout_(nullptr), nh_(nh)
{
    
}

ScoutROSMessenger::ScoutROSMessenger(ScoutBase *scout, ros::NodeHandle *nh) : scout_(scout), nh_(nh)
{
    manager = LpmsSensorManagerFactory();
    std::string imu_port;
    ros::NodeHandle nh2("~");
    nh2.param<std::string>("imu_port", imu_port, std::string("ttyUSB1"));
    nh2.param<double>("turn_gain", turn_gain, 1.0);
    nh2.param<double>("safety_x", safety_x, 0.55);
    nh2.param<double>("safety_y", safety_y, 0.3);
    nh2.param<bool>("is_emergent_stop", is_emergent_stop, false);
    ROS_INFO("turn gain:%f",turn_gain);
    std::string port_imu_str = "/dev/"+imu_port;
	lpms = manager->addSensor(DEVICE_LPMS_RS232, port_imu_str.c_str());
    imu_publisher_ = nh_->advertise<sensor_msgs::Imu>("/imu",2);
    std::thread t1(&ScoutROSMessenger::getImuData,this);
    t1.detach();

}

ScoutROSMessenger::~ScoutROSMessenger()
{
    if(manager != nullptr)
    {
        manager->removeSensor(lpms);
        delete manager;
    }
}

void ScoutROSMessenger::getImuData()
{
    ros::Rate r(50);
    while(ros::ok())
    {
        if (lpms->getConnectionStatus() == SENSOR_CONNECTION_CONNECTED &&            
		lpms->hasImuData()) 
        {
            // Reads quaternion data
            ImuData d = lpms->getCurrentData();

            imuMsg.header.frame_id = "imu_link";
            imuMsg.header.stamp = ros::Time::now();
            imuMsg.linear_acceleration.x =  d.linAcc[0]*GRAVITY_VALUE;
            imuMsg.linear_acceleration.y =  d.linAcc[1]*GRAVITY_VALUE;
            imuMsg.linear_acceleration.z =  d.linAcc[2]*GRAVITY_VALUE;

            imuMsg.angular_velocity.x = d.w[0]/57.3;
            imuMsg.angular_velocity.y = d.w[1]/57.3;
            imuMsg.angular_velocity.z = d.w[2]/57.3;;

            imuMsg.orientation.x = d.q[1];
            imuMsg.orientation.y = d.q[2];
            imuMsg.orientation.z = d.q[3];
            imuMsg.orientation.w = d.q[0];
            imu_publisher_.publish(imuMsg);
        }
        r.sleep();
    }
}

void ScoutROSMessenger::SetupSubscription()
{
    // odometry publisher
    odom_publisher_ = nh_->advertise<nav_msgs::Odometry>(odom_frame_, 50);
    status_publisher_ = nh_->advertise<scout_msgs::ScoutStatus>("/scout_status", 10);

    // cmd subscriber
    motion_cmd_subscriber_ = nh_->subscribe<geometry_msgs::Twist>("/smoother_cmd_vel", 5, &ScoutROSMessenger::TwistCmdCallback, this); //不启用平滑包则订阅“cmd_vel”
    light_cmd_subscriber_ = nh_->subscribe<scout_msgs::ScoutLightCmd>("/scout_light_control", 5, &ScoutROSMessenger::LightCmdCallback, this);

    laser_subscriber_ = nh_->subscribe<sensor_msgs::LaserScan>("/laser_scan", 2, &ScoutROSMessenger::emergencyLaserScanCallback, this);

}

void ScoutROSMessenger::TwistCmdCallback(const geometry_msgs::Twist::ConstPtr &msg)
{
    if (!simulated_robot_)
    {
        double vel_x = msg->linear.x;
        double vel_th = msg->angular.z;
//        ROS_INFO("vel:(%f,%f)",vel_x,vel_th);
        if(is_emergent_stop)
        {
 //           ROS_INFO("enable emergen stop ");
            if(vel_x > 0)
                vel_x = front_blocked_ ? 0.0 : vel_x;
            else
                vel_x = back_blocked_ ? 0.0 : vel_x; 

            vel_th = front_blocked_ || back_blocked_ ? 0.0 : vel_th;
        }
//        ROS_INFO("vel:(%f,%f)",vel_x,vel_th);
        scout_->SetMotionCommand(vel_x, vel_th*turn_gain);
        //scout_->SetMotionCommand(0.0,0.0);
    }
    else
    {
        std::lock_guard<std::mutex> guard(twist_mutex_);
        current_twist_ = *msg.get();
    }
    // ROS_INFO("cmd received:%f, %f", msg->linear.x, msg->angular.z);
}

void ScoutROSMessenger::GetCurrentMotionCmdForSim(double &linear, double &angular)
{
    std::lock_guard<std::mutex> guard(twist_mutex_);
    linear = current_twist_.linear.x;
    angular = current_twist_.angular.z;
}

void ScoutROSMessenger::LightCmdCallback(const scout_msgs::ScoutLightCmd::ConstPtr &msg)
{
    if (!simulated_robot_)
    {
        if (msg->enable_cmd_light_control)
        {
            ScoutLightCmd cmd;

            switch (msg->front_mode)
            {
            case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF:
            {
                cmd.front_mode = ScoutLightCmd::LightMode::CONST_OFF;
                break;
            }
            case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON:
            {
                cmd.front_mode = ScoutLightCmd::LightMode::CONST_ON;
                break;
            }
            case scout_msgs::ScoutLightCmd::LIGHT_BREATH:
            {
                cmd.front_mode = ScoutLightCmd::LightMode::BREATH;
                break;
            }
            case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM:
            {
                cmd.front_mode = ScoutLightCmd::LightMode::CUSTOM;
                cmd.front_custom_value = msg->front_custom_value;
                break;
            }
            }

            switch (msg->rear_mode)
            {
            case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF:
            {
                cmd.rear_mode = ScoutLightCmd::LightMode::CONST_OFF;
                break;
            }
            case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON:
            {
                cmd.rear_mode = ScoutLightCmd::LightMode::CONST_ON;
                break;
            }
            case scout_msgs::ScoutLightCmd::LIGHT_BREATH:
            {
                cmd.rear_mode = ScoutLightCmd::LightMode::BREATH;
                break;
            }
            case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM:
            {
                cmd.rear_mode = ScoutLightCmd::LightMode::CUSTOM;
                cmd.rear_custom_value = msg->rear_custom_value;
                break;
            }
            }

            scout_->SetLightCommand(cmd);
        }
        else
        {
            scout_->DisableLightCmdControl();
        }
    }
    else
    {
        std::cout << "simulated robot received light control cmd" << std::endl;
    }
}

void ScoutROSMessenger::PublishStateToROS()
{
    current_time_ = ros::Time::now();
    double dt = (current_time_ - last_time_).toSec();

    static bool init_run = true;
    if (init_run)
    {
        last_time_ = current_time_;
        init_run = false;
        return;
    }

    auto state = scout_->GetScoutState();

    // publish scout state message
    scout_msgs::ScoutStatus status_msg;

    status_msg.header.stamp = current_time_;

    status_msg.linear_velocity = state.linear_velocity;
    status_msg.angular_velocity = imuMsg.angular_velocity.z;//state.angular_velocity;

    status_msg.base_state = state.base_state;
    status_msg.control_mode = state.control_mode;
    status_msg.fault_code = state.fault_code;
    status_msg.battery_voltage = state.battery_voltage;

    for (int i = 0; i < 4; ++i)
    {
        status_msg.motor_states[i].current = state.motor_states[i].current;
        status_msg.motor_states[i].rpm = state.motor_states[i].rpm;
        status_msg.motor_states[i].temperature = state.motor_states[i].temperature;
    }

    status_msg.light_control_enabled = state.light_control_enabled;
    status_msg.front_light_state.mode = state.front_light_state.mode;
    status_msg.front_light_state.custom_value = state.front_light_state.custom_value;
    status_msg.rear_light_state.mode = state.rear_light_state.mode;
    status_msg.rear_light_state.custom_value = state.front_light_state.custom_value;

    status_publisher_.publish(status_msg);

    // publish odometry and tf
    PublishOdometryToROS(state.linear_velocity, imuMsg.angular_velocity.z, dt);

    // record time for next integration
    last_time_ = current_time_;
}

void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular)
{
    current_time_ = ros::Time::now();
    double dt = 1.0 / sim_control_rate_;

    // publish scout state message
    scout_msgs::ScoutStatus status_msg;

    status_msg.header.stamp = current_time_;

    status_msg.linear_velocity = linear;
    status_msg.angular_velocity = angular;

    status_msg.base_state = 0x00;
    status_msg.control_mode = 0x01;
    status_msg.fault_code = 0x00;
    status_msg.battery_voltage = 29.5;

    // for (int i = 0; i < 4; ++i)
    // {
    //     status_msg.motor_states[i].current = state.motor_states[i].current;
    //     status_msg.motor_states[i].rpm = state.motor_states[i].rpm;
    //     status_msg.motor_states[i].temperature = state.motor_states[i].temperature;
    // }

    status_msg.light_control_enabled = false;
    // status_msg.front_light_state.mode = state.front_light_state.mode;
    // status_msg.front_light_state.custom_value = state.front_light_state.custom_value;
    // status_msg.rear_light_state.mode = state.rear_light_state.mode;
    // status_msg.rear_light_state.custom_value = state.front_light_state.custom_value;

    status_publisher_.publish(status_msg);

    // publish odometry and tf
    PublishOdometryToROS(linear, angular, dt);
}

void ScoutROSMessenger::PublishOdometryToROS(double linear, double angular, double dt)
{
    // perform numerical integration to get an estimation of pose
    linear_speed_ = linear;
    angular_speed_ = angular;

    double d_x = linear_speed_ * std::cos(theta_) * dt;
    double d_y = linear_speed_ * std::sin(theta_) * dt;
    double d_theta = angular_speed_ * dt;

    position_x_ += d_x;
    position_y_ += d_y;
    theta_ += d_theta;

    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(theta_);

    // publish tf transformation
    geometry_msgs::TransformStamped tf_msg;
    tf_msg.header.stamp = current_time_;
    tf_msg.header.frame_id = odom_frame_;
    tf_msg.child_frame_id = base_frame_;

    tf_msg.transform.translation.x = position_x_;
    tf_msg.transform.translation.y = position_y_;
    tf_msg.transform.translation.z = 0.0;
    tf_msg.transform.rotation = odom_quat;

    //tf_broadcaster_.sendTransform(tf_msg);

    // publish odometry and tf messages
    nav_msgs::Odometry odom_msg;
    odom_msg.header.stamp = current_time_;
    odom_msg.header.frame_id = odom_frame_;
    odom_msg.child_frame_id = base_frame_;

    odom_msg.pose.pose.position.x = position_x_;
    odom_msg.pose.pose.position.y = position_y_;
    odom_msg.pose.pose.position.z = 0.0;
    odom_msg.pose.pose.orientation = odom_quat;

    odom_msg.twist.twist.linear.x = linear_speed_;
    odom_msg.twist.twist.linear.y = 0.0;
    odom_msg.twist.twist.angular.z = angular_speed_;

    odom_publisher_.publish(odom_msg);
}
void ScoutROSMessenger::emergencyLaserScanCallback(const sensor_msgs::LaserScan::ConstPtr& raw_message)
{
  float epsilon = 0.0001;  // a tenth of a millimeter
  sensor_msgs::LaserScan message = *raw_message;
  for (size_t i = 0; i < message.ranges.size(); i++)
  {
    float range = message.ranges[ i ];
    if (!std::isfinite(range) && range > 0)
    {
      message.ranges[ i ] = message.range_max - epsilon;
    }
  }
  laser_geometry::LaserProjection projector_;
  // project the laser into a point cloud
  sensor_msgs::PointCloud2 cloud;
  cloud.header = message.header;

  // project the scan into a point cloud
  try
  {
    projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, tf_);
  }
  catch (tf::TransformException &ex)
  {
    ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame");
    projector_.projectLaser(message, cloud);
  }

  pcl::PointCloud < pcl::PointXYZ > pcl_cloud;
  pcl_cloud.header.frame_id = cloud.header.frame_id;
  try
  {
    pcl::PCLPointCloud2 pcl_pc2;
    pcl_conversions::toPCL(cloud, pcl_pc2);
    // Actually convert the PointCloud2 message into a type we can reason about
    pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud);
  }
  catch (pcl::PCLException& ex)
  {
    ROS_ERROR("Failed to convert a message to a pcl type, dropping observation: %s", ex.what());
	back_blocked_ = true;
    front_blocked_ = true;
    return ;
  }
  emergent_judge(pcl_cloud);
}
void ScoutROSMessenger::emergent_judge(pcl::PointCloud < pcl::PointXYZ > pcl_cloud)
{
  pcl::PointCloud < pcl::PointXYZ > global_frame_cloud;

  // transform the point cloud

  pcl_ros::transformPointCloud(robot_base_frame_, pcl_cloud, global_frame_cloud, tf_);

  unsigned int cloud_size = global_frame_cloud.points.size();
  unsigned int point_count = 0;

  //float safetyRadius_pow = safetyRadius*safetyRadius; 
//  ROS_INFO("cloud_size:%d",cloud_size);
    bool front_flag = false;
    bool back_flag = false; 
  for (unsigned int i = 0; i < cloud_size; ++i)
  {
    if(std::fabs(global_frame_cloud.points[i].y) < safety_y + 0.05)
    {
        if(global_frame_cloud.points[i].x > 0.2 && global_frame_cloud.points[i].x < safety_x )
        {
            front_flag = true;
//            ROS_INFO("front blocked");
        }
        else if(global_frame_cloud.points[i].x < -0.2 && global_frame_cloud.points[i].x > -safety_x)
        {
//            ROS_INFO("back blocked");
            back_flag = true;
        }
    }
  }

  front_blocked_ = front_flag;	
  back_blocked_ = back_flag;

}
} // namespace wescore